# beihang_converter.py
"""
北航格式转换器
专门用于处理北航航线规划接口格式的转换
"""
from typing import Dict, List, Any, Tuple
from datetime import datetime, timedelta
import json
from shapely.geometry import Point, Polygon, MultiPolygon, LineString
from shapely import wkt
from algorithm.coord_transform import PositionConvert
from FastAPI.beihang_models import BeihangRoutePlanRequest, BeihangRoutePlanResponse
from FastAPI.logger_config import get_logger

logger = get_logger()

class BeihangConverter:
    """北航格式转换器"""
    
    def __init__(self):
        self.pos_converter = PositionConvert()
    
    def beihang_to_internal(self, beihang_request: Dict) -> Dict:
        """
        将北航格式转换为内部格式
        """
        try:
            logger.info("[北航转换器] 开始转换北航格式为内部格式")
            
            # 验证输入格式
            if not self._validate_beihang_request(beihang_request):
                raise ValueError("北航请求格式验证失败")
            
            # 提取环境信息并转换为地图边界
            env_info = beihang_request.get('environmentalInfo', {})
            map_bounds = self._extract_map_bounds(env_info)
            
            # 构造任务列表
            tasks = self._create_tasks_from_beihang(beihang_request)
            
            # 构造配置
            config = self._create_config(beihang_request, map_bounds)
            
            result = {
                'tasks': tasks,
                'config': config
            }
            
            logger.info(f"[北航转换器] 转换完成，生成 {len(tasks)} 个任务")
            return result
            
        except Exception as e:
            logger.error(f"[北航转换器] 转换失败: {e}")
            raise
    
    def internal_to_beihang(self, internal_response: Dict, beihang_request: Dict) -> Dict:
        """
        将内部格式转换为北航格式
        """
        try:
            logger.info("[北航转换器] 开始转换内部格式为北航格式")
            
            if internal_response.get('status') != 'success':
                logger.warning("[北航转换器] 内部响应状态不为success，返回空结果")
                return {
                    'routeInfos': [],
                    'extraInfo': beihang_request.get('extraInfo', {})
                }
            
            data = internal_response.get('data', {})
            trajectories = data.get('trajectories', {})
            time_paths = data.get('time_paths', {})
            
            route_infos = []
            
            # 处理每个设备的轨迹
            for device_id_str, trajectory_data in trajectories.items():
                device_id = int(device_id_str)
                trajectory = trajectory_data.get('trajectory', [])
                
                if not trajectory:
                    logger.warning(f"[北航转换器] 设备 {device_id} 没有轨迹数据")
                    continue
                
                # 转换轨迹点为北航格式
                route_points = self._convert_trajectory_to_route_points(
                    trajectory, time_paths, device_id_str, trajectory_data
                )
                
                # 计算航线统计信息
                stats = self._calculate_route_stats(trajectory)
                
                # 生成业务标识
                out_biz_code = f"{beihang_request.get('outBizCode', '')}_{device_id}"
                
                # 计算时间范围
                start_time = beihang_request.get('startTime') or datetime.now()
                if isinstance(start_time, str):
                    start_time = datetime.fromisoformat(start_time.replace('Z', '+00:00'))
                end_time = self._calculate_end_time(start_time, time_paths, device_id_str)
                
                # 获取高度信息
                max_alt = int(beihang_request.get('extraInfo', {}).get('routeHeight', stats['max_alt']))
                min_alt = int(beihang_request.get('extraInfo', {}).get('routeHeight', stats['min_alt']))
                
                route_info = {
                    'outBizCode': out_biz_code,
                    'routePoints': route_points,
                    'startTime': start_time.isoformat(),
                    'endTime': end_time.isoformat(),
                    'maxAlt': max_alt,
                    'minAlt': min_alt,
                    'width': 20,  # 默认扫描宽度
                    'length': round(stats['length'], 2)
                }
                
                route_infos.append(route_info)
                logger.info(f"[北航转换器] 设备 {device_id} 轨迹转换完成，{len(route_points)} 个航点")
            
            result = {
                'routeInfos': route_infos,
                'extraInfo': beihang_request.get('extraInfo', {})
            }
            
            logger.info(f"[北航转换器] 转换完成，生成 {len(route_infos)} 条航线")
            return result
            
        except Exception as e:
            logger.error(f"[北航转换器] 转换失败: {e}")
            return {
                'routeInfos': [],
                'extraInfo': beihang_request.get('extraInfo', {})
            }
    
    def _validate_beihang_request(self, request: Dict) -> bool:
        """
        验证北航请求格式
        """
        required_fields = ['deviceInfos', 'deviceModel', 'outBizCode', 'routeType', 'environmentalInfo']
        for field in required_fields:
            if field not in request:
                logger.error(f"[北航转换器] 缺少必需字段: {field}")
                return False
        
        # 验证设备信息
        device_infos = request.get('deviceInfos', [])
        if not device_infos:
            logger.error("[北航转换器] 设备信息列表为空")
            return False
        
        # 验证航线类型
        route_type = request.get('routeType')
        if route_type not in ['point', 'area']:
            logger.error(f"[北航转换器] 不支持的航线类型: {route_type}")
            return False
        
        logger.info("[北航转换器] 请求格式验证通过")
        return True
    
    def _extract_map_bounds(self, env_info: Dict) -> List[float]:
        """
        从环境信息中提取地图边界
        """
        # 使用经过验证的默认边界，确保算法正常工作
        # 在实际应用中，可以根据环境数据动态计算边界
        default_bounds = [-300, 0, 400, 400]
        
        try:
            # 尝试从环境数据中提取边界
            all_coords = []
            
            # 处理障碍物
            for obstacle_wkt in env_info.get('obstacles', []):
                geom = wkt.loads(obstacle_wkt)
                if geom.geom_type == 'Polygon':
                    all_coords.extend(list(geom.exterior.coords))
                elif geom.geom_type == 'MultiPolygon':
                    for poly in geom.geoms:
                        all_coords.extend(list(poly.exterior.coords))
            
            # 处理禁飞区
            for no_fly_wkt in env_info.get('noFlyZones', []):
                geom = wkt.loads(no_fly_wkt)
                if geom.geom_type == 'Polygon':
                    all_coords.extend(list(geom.exterior.coords))
            
            if all_coords:
                # 计算边界
                lons = [coord[0] for coord in all_coords]
                lats = [coord[1] for coord in all_coords]
                
                # 转换为本地坐标计算范围
                ref_lat = 29.35541411
                ref_lon = 106.0073147
                
                local_coords = []
                for lon, lat in zip(lons, lats):
                    x, y = self.pos_converter.GPStoXY(lat, lon, ref_lat, ref_lon)
                    local_coords.append((x, y))
                
                if local_coords:
                    xs = [coord[0] for coord in local_coords]
                    ys = [coord[1] for coord in local_coords]
                    
                    # 添加边距
                    x_margin = (max(xs) - min(xs)) * 0.2
                    y_margin = (max(ys) - min(ys)) * 0.2
                    
                    bounds = [
                        min(xs) - x_margin,
                        min(ys) - y_margin,
                        max(xs) + x_margin,
                        max(ys) + y_margin
                    ]
                    
                    logger.info(f"[北航转换器] 从环境数据计算边界: {bounds}")
                    return bounds
        
        except Exception as e:
            logger.warning(f"[北航转换器] 从环境数据提取边界失败，使用默认边界: {e}")
        
        logger.info(f"[北航转换器] 使用默认边界: {default_bounds}")
        return default_bounds
    
    def _create_tasks_from_beihang(self, beihang_request: Dict) -> List[Dict]:
        """
        从北航请求创建内部任务
        """
        tasks = []
        route_type = beihang_request.get('routeType', 'point')
        
        if route_type == 'area':
            # 面航线 - 转换为patrol任务
            task = self._create_patrol_task_from_beihang(beihang_request)
            tasks.append(task)
        else:
            # 点航线 - 转换为waypoint任务
            task = self._create_waypoint_task_from_beihang(beihang_request)
            tasks.append(task)
        
        return tasks
    
    def _create_patrol_task_from_beihang(self, beihang_request: Dict) -> Dict:
        """
        从北航请求创建巡航任务
        支持标准巡航区域格式，使用多边形顶点定义巡逻区域
        """
        device_infos = beihang_request.get('deviceInfos', [])
        uav_ids = [info['deviceId'] for info in device_infos]
        
        # 获取起始点
        start_points = []
        for device_info in device_infos:
            start_point = beihang_request.get('startPoint')
            if start_point:
                start_points.append({
                    'uav_id': device_info['deviceId'],
                    'position': {
                        'lon': start_point['longitude'],
                        'lat': start_point['latitude'],
                        'alt': start_point['altitude']
                    }
                })
        
        # 构建巡逻区域 - 支持多种格式
        vertices = []
        
        # 格式1: 使用patrolArea字段（推荐）
        patrol_area = beihang_request.get('patrolArea')
        if patrol_area:
            vertices = self._parse_patrol_area(patrol_area)
        
        # 格式2: 使用wayPoints字段（兼容旧格式）
        if not vertices:
            way_points = beihang_request.get('wayPoints', [])
            for point in way_points:
                vertices.append({
                    'lon': point['longitude'],
                    'lat': point['latitude'],
                    'alt': point.get('altitude', 40.0)
                })
        
        # 格式3: 使用环境信息中的敏感区域
        if not vertices:
            vertices = self._extract_patrol_area_from_environment(beihang_request)
        
        # 如果没有定义区域，创建默认巡逻区域
        if not vertices:
            vertices = self._create_default_patrol_area(beihang_request)
        
        logger.info(f"[北航转换器] 创建巡航任务，巡逻区域顶点数: {len(vertices)}")
        
        return {
            'priority': 1,
            'taskType': 'patrol',
            'time_constraint': beihang_request.get('extraInfo', {}).get('maxFlightTime', 1000),
            'uav_ids': uav_ids,
            'start': start_points,
            'vertices': vertices
        }
    
    def _create_waypoint_task_from_beihang(self, beihang_request: Dict) -> Dict:
        """
        从北航请求创建航点任务
        """
        device_infos = beihang_request.get('deviceInfos', [])
        uav_ids = [info['deviceId'] for info in device_infos]
        
        # 获取起始点
        start_points = []
        for device_info in device_infos:
            start_point = beihang_request.get('startPoint')
            if start_point:
                start_points.append({
                    'uav_id': device_info['deviceId'],
                    'position': {
                        'lon': start_point['longitude'],
                        'lat': start_point['latitude'],
                        'alt': start_point['altitude']
                    }
                })
        
        # 获取目标点
        goals = []
        end_point = beihang_request.get('endPoint')
        if end_point:
            for device_info in device_infos:
                goals.append({
                    'uav_id': device_info['deviceId'],
                    'position': {
                        'lon': end_point['longitude'],
                        'lat': end_point['latitude'],
                        'alt': end_point['altitude']
                    }
                })
        else:
            # 如果没有终点，使用途径点作为目标
            way_points = beihang_request.get('wayPoints', [])
            if way_points:
                last_point = way_points[-1]
                for device_info in device_infos:
                    goals.append({
                        'uav_id': device_info['deviceId'],
                        'position': {
                            'lon': last_point['longitude'],
                            'lat': last_point['latitude'],
                            'alt': last_point.get('altitude', 40.0)
                        }
                    })
        
        return {
            'priority': 1,
            'taskType': 'waypoint',
            'time_constraint': beihang_request.get('extraInfo', {}).get('maxFlightTime', 600),
            'uav_ids': uav_ids,
            'start': start_points,
            'goals': goals
        }
    
    def _create_default_patrol_area(self, beihang_request: Dict) -> List[Dict]:
        """
        创建默认巡逻区域
        """
        start_point = beihang_request.get('startPoint')
        if start_point:
            # 在起始点周围创建一个矩形区域
            center_lon = start_point['longitude']
            center_lat = start_point['latitude']
            altitude = start_point.get('altitude', 40.0)
            
            # 创建100m x 100m的矩形区域
            offset = 0.001  # 大约100米的偏移
            
            return [
                {'lon': center_lon - offset, 'lat': center_lat - offset, 'alt': altitude},
                {'lon': center_lon + offset, 'lat': center_lat - offset, 'alt': altitude},
                {'lon': center_lon + offset, 'lat': center_lat + offset, 'alt': altitude},
                {'lon': center_lon - offset, 'lat': center_lat + offset, 'alt': altitude}
            ]
        
        # 默认区域
        return [
            {'lon': 106.009, 'lat': 29.358, 'alt': 40.0},
            {'lon': 106.010, 'lat': 29.358, 'alt': 40.0},
            {'lon': 106.010, 'lat': 29.357, 'alt': 40.0},
            {'lon': 106.009, 'lat': 29.357, 'alt': 40.0}
        ]
    
    def _create_config(self, beihang_request: Dict, map_bounds: List[float]) -> Dict:
        """
        创建配置
        """
        # 尝试从环境数据中提取参考点
        env_info = beihang_request.get('environmentalInfo', {})
        ref_lat = 29.35541411
        ref_lon = 106.0073147
        
        try:
            # 从起始点获取参考坐标
            start_point = beihang_request.get('startPoint')
            if start_point:
                ref_lat = start_point['latitude']
                ref_lon = start_point['longitude']
        except Exception as e:
            logger.warning(f"[北航转换器] 获取参考点失败，使用默认值: {e}")
        
        return {
            'ref_lat': ref_lat,
            'ref_lon': ref_lon,
            'robot_radius': 2.0,
            'coarse_grid_size': 20.0,
            'fine_grid_size': 5.0,
            'map_bounds': map_bounds
        }
    
    def _convert_trajectory_to_route_points(self, trajectory: List[Dict], time_paths: Dict, device_id: str, trajectory_data: Dict) -> List[Dict]:
        """
        转换轨迹为北航格式的航点
        """
        route_points = []
        
        for i, point in enumerate(trajectory):
            # 计算时间戳
            timestamp = self._calculate_timestamp(time_paths, device_id, i)
            
            # 获取速度
            speed = trajectory_data.get('suggested_planning_speed', 8.0)
            
            route_point = {
                'longitude': point['lon'],
                'latitude': point['lat'],
                'altitude': point['alt'],
                'speed': round(speed, 2),
                'timestamp': timestamp,
                'pitch': 0.0,  # 默认值
                'roll': 0.0,   # 默认值
                'yaw': 0.0,    # 默认值
                'xVelocity': 0.0,  # 默认值
                'yVelocity': 0.0,  # 默认值
                'zVelocity': 0.0   # 默认值
            }
            route_points.append(route_point)
        
        return route_points
    
    def _calculate_timestamp(self, time_paths: Dict, device_id: str, point_index: int) -> int:
        """
        计算时间戳
        """
        device_path = time_paths.get(device_id, [])
        if point_index < len(device_path):
            # 获取基础时间
            base_time = device_path[point_index][2]
            # 转换为毫秒时间戳
            return int(base_time * 1000)
        return 0
    
    def _calculate_route_stats(self, trajectory: List[Dict]) -> Dict:
        """
        计算航线统计信息
        """
        if not trajectory:
            return {'max_alt': 0, 'min_alt': 0, 'length': 0}
        
        alts = [point['alt'] for point in trajectory]
        max_alt = max(alts)
        min_alt = min(alts)
        
        # 计算航线长度
        length = 0
        for i in range(1, len(trajectory)):
            p1, p2 = trajectory[i-1], trajectory[i]
            
            # 使用坐标转换器计算实际距离
            try:
                x1, y1 = self.pos_converter.GPStoXY(p1['lat'], p1['lon'], 29.35541411, 106.0073147)
                x2, y2 = self.pos_converter.GPStoXY(p2['lat'], p2['lon'], 29.35541411, 106.0073147)
                distance = ((x2 - x1)**2 + (y2 - y1)**2)**0.5
                length += distance
            except Exception as e:
                logger.warning(f"[北航转换器] 距离计算失败: {e}")
                # 使用简化的距离计算
                dx = p2['lon'] - p1['lon']
                dy = p2['lat'] - p1['lat']
                length += (dx**2 + dy**2)**0.5 * 111000
        
        return {
            'max_alt': max_alt,
            'min_alt': min_alt,
            'length': length
        }
    
    def _calculate_end_time(self, start_time: datetime, time_paths: Dict, device_id: str) -> datetime:
        """
        计算结束时间
        """
        device_path = time_paths.get(device_id, [])
        if device_path:
            total_seconds = device_path[-1][2] if device_path else 0
            return start_time + timedelta(seconds=total_seconds)
        return start_time
    
    def _parse_patrol_area(self, patrol_area: Dict) -> List[Dict]:
        """
        解析标准巡航区域格式
        """
        vertices = []
        
        # 格式1: 多边形顶点列表
        if 'vertices' in patrol_area:
            for vertex in patrol_area['vertices']:
                vertices.append({
                    'lon': vertex['longitude'],
                    'lat': vertex['latitude'],
                    'alt': vertex.get('altitude', 40.0)
                })
        
        # 格式2: WKT格式
        elif 'wkt' in patrol_area:
            try:
                geom = wkt.loads(patrol_area['wkt'])
                if geom.geom_type == 'Polygon':
                    for coord in geom.exterior.coords:
                        vertices.append({
                            'lon': coord[0],
                            'lat': coord[1],
                            'alt': patrol_area.get('altitude', 40.0)
                        })
            except Exception as e:
                logger.warning(f"[北航转换器] 解析WKT格式失败: {e}")
        
        # 格式3: 边界框格式
        elif 'bounds' in patrol_area:
            bounds = patrol_area['bounds']
            vertices = [
                {'lon': bounds['minLon'], 'lat': bounds['minLat'], 'alt': bounds.get('altitude', 40.0)},
                {'lon': bounds['maxLon'], 'lat': bounds['minLat'], 'alt': bounds.get('altitude', 40.0)},
                {'lon': bounds['maxLon'], 'lat': bounds['maxLat'], 'alt': bounds.get('altitude', 40.0)},
                {'lon': bounds['minLon'], 'lat': bounds['maxLat'], 'alt': bounds.get('altitude', 40.0)}
            ]
        
        return vertices
    
    def _extract_patrol_area_from_environment(self, beihang_request: Dict) -> List[Dict]:
        """
        从环境信息中提取巡逻区域
        """
        vertices = []
        env_info = beihang_request.get('environmentalInfo', {})
        
        # 从敏感区域提取巡逻区域
        sensitive_areas = env_info.get('sensitiveAreas', [])
        for area in sensitive_areas:
            if area.get('type') in ['patrol', 'surveillance']:
                try:
                    geom = wkt.loads(area['geom'])
                    if geom.geom_type == 'Polygon':
                        for coord in geom.exterior.coords:
                            vertices.append({
                                'lon': coord[0],
                                'lat': coord[1],
                                'alt': area.get('altitude', 40.0)
                            })
                        break  # 只使用第一个找到的巡逻区域
                except Exception as e:
                    logger.warning(f"[北航转换器] 解析敏感区域失败: {e}")
        
        return vertices